#ifndef __CLD_REG_H__
#define __CLD_REG_H__

#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <pcl/PointIndices.h>
#include <pcl/correspondence.h>

#include "ouster_point_type.h"

namespace cldreg {

//using PointType = pcl::PointXYZ;
using PointType = PointOuster;

class Reg {

public:
	Reg(pcl::PointCloud<PointType>::Ptr& cldptr_bg, double d_resolution, int n_corr_max);
	//void init();
	bool getFinalTransformation(pcl::PointCloud<PointType>::Ptr& cldptr_raw, Eigen::Matrix4f& mat_res);

	void prepCld(	pcl::PointCloud<PointType>::Ptr& cldptr_src, 
					double d_resolution, 
					pcl::PointCloud<PointType>::Ptr& cldptr_ds, 
					pcl::PointCloud<PointType>::Ptr& cldptr_iss, 
					pcl::PointIndicesPtr& indptr_iss, 
					pcl::PointCloud<pcl::FPFHSignature33>::Ptr& cldptr_fpfh);

	void correspondenceSearch(	pcl::PointCloud<pcl::FPFHSignature33>::Ptr& cldptr_src_fpfh, 
								pcl::PointCloud<pcl::FPFHSignature33>::Ptr& cldptr_tgt_fpfh, 
								pcl::CorrespondencesPtr& corrptr, 
								const int n_corr_max, 
								std::vector<int>& vec_corr_src, 
								std::vector<int>& vec_corr_tgt);

	void grorReg(	pcl::PointCloud<PointType>::Ptr& cldptr_src, 
					pcl::PointCloud<PointType>::Ptr& cldptr_tgt, 
					pcl::CorrespondencesPtr& corrptr, 
					const double d_resolution, 
					pcl::PointCloud<PointType>::Ptr& cldptr_gror, 
					Eigen::Matrix4f& transform_gror, 
					double& score_gror);

	void gicpReg(	pcl::PointCloud<PointType>::Ptr& cldptr_src, 
					pcl::PointCloud<PointType>::Ptr& cldptr_tgt, 
					Eigen::Matrix4f& transform_init, 
					pcl::PointCloud<PointType>::Ptr& cldptr_gicp, 
					Eigen::Matrix4f& transform_gicp, 
					double& score_gicp);

private:
	double d_resolution_;
	int n_corr_max_;

	pcl::PointCloud<PointType>::Ptr cldptr_bg_ds_;
	pcl::PointCloud<PointType>::Ptr cldptr_bg_iss_;
	pcl::PointIndicesPtr indptr_bg_iss_;
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr cldptr_bg_fpfh_;
};  // class Reg

}  // namespace cldreg

#endif  // __CLD_REG_H__

